#include <stdio.h>
#include <string.h>
#include <aos/kernel.h>
#include <ulog/ulog.h>
#include <math.h>
#define SMXUSBD
#if defined(SMXUSBD)
#include "smxbase.h"
#include "smxusbd.h"
#endif
//#include "yuv422_160x120.h"
//#include "ipcm_function.h"
//#include "ipcm.h"
#include "link_queue.h"
#include "packet.h"
//#include "net_param.h"
#include "faceproc.h"
#include "face_result.h"
#include "mm_config.h"
#include "usbddemo.h"
#include "image_proc.h"
#include "img_cli_deal.h"
#include "para_traverse_task.h"
#include "usbdtsm.h"
#include "udftempl.h"
#include "usbddemo.h"
//#include "imageusbtask/usbdtsm.h"
#include "image_task.h"
//#include "aie_task.h"
#include "drv_wdt.h"
#include "uart_packet.h"
#include "data_move.h"
#include "if_v.h"

#define USB_TRANS_DEB_LOG 0

#if USB_TRANS_DEB_LOG
#define usb_trans_log printf
#else
#define usb_trans_log(...)
#endif

extern uint32_t g_snapshot_done;
int g_depth_image_status = 0; //1: enable get //0:get success

#define RGBA_TO_BGRA(color)       (((color & 0XFF00FF00) | (color >>16)&0XFF) | ((color & 0XFF) <<16))

#define USB_PORT 0
#define SUD_FTEMPL_BULKIN_PKTSIZE  512
#define SUD_FTEMPL_BULKOUT_PKTSIZE  512
#define SUD_FTEMPL_BULKIN_QUEUE_SIZE  50  
#define SUD_FTEMPL_BULKOUT_QUEUE_SIZE ((1920*1080*4)/SUD_FTEMPL_BULKOUT_PKTSIZE +1) //~8M

#define DEPTH_IMAGE_CH 1
#define DEPTH_IMAGE_HEIGHT 360
#define DEPTH_IMAGE_WIDTH 640
#define DEPTH_IMAGE_PIXEL_SIZE 2 //short
#define DEPTH_IMAGE_SIZE  (DEPTH_IMAGE_CH * DEPTH_IMAGE_HEIGHT * DEPTH_IMAGE_WIDTH * DEPTH_IMAGE_PIXEL_SIZE)

#define IR_IMAGE_CH 1
#define IR_IMAGE_HEIGHT 360
#define IR_IMAGE_WIDTH 640
#define IR_IMAGE_PIXEL_SIZE 1 //8bit
#define IR_IMAGE_SIZE  (IR_IMAGE_CH * IR_IMAGE_HEIGHT *IR_IMAGE_WIDTH * IR_IMAGE_PIXEL_SIZE)

#define RGB_IMAGE_CH 4
#define RGB_IMAGE_HEIGHT 360
#define RGB_IMAGE_WIDTH 640
#define RGB_IMAGE_PIXEL_SIZE 1 //8bit
#define RGB_IMAGE_SIZE  (RGB_IMAGE_CH * RGB_IMAGE_HEIGHT * RGB_IMAGE_WIDTH * RGB_IMAGE_PIXEL_SIZE)

#define SGL_IR_IMAGE_CH 1
#define SGL_IR_IMAGE_HEIGHT 720
#define SGL_IR_IMAGE_WIDTH 1280
#define SGL_IR_IMAGE_PIXEL_SIZE 2 //short
#define SGL_IR_IMAGE_SIZE (SGL_IR_IMAGE_CH * SGL_IR_IMAGE_HEIGHT * SGL_IR_IMAGE_WIDTH * SGL_IR_IMAGE_PIXEL_SIZE)

#define SGL_RGB_IMAGE_CH 2
#define SGL_RGB_IMAGE_HEIGHT 1080
#define SGL_RGB_IMAGE_WIDTH 1920
#define SGL_RGB_IMAGE_PIXEL_SIZE 1 //8bit
#define SGL_RGB_IMAGE_SIZE  (SGL_RGB_IMAGE_CH * SGL_RGB_IMAGE_HEIGHT * SGL_RGB_IMAGE_WIDTH * SGL_RGB_IMAGE_PIXEL_SIZE)


#define SET_USB_CTRL_SATUE(cmd) {\
	UsbTsmPrivate.usb_cmd = cmd; \
	UsbTsmPrivate.get_image = true; \
    UsbTsmPrivate.ai_task_ctl = true; \
    UsbTsmPrivate.aie_cal_flag = false; \
}

////#define PICTURE_DECODE_CHAR_LEN   921600
////#define FACE_FEATURE_LEN 384
////#define FACE_FEATURE_MAX_NUM 25000
////#define FACE_FEATURE_SIZE  (FACE_FEATURE_MAX_NUM * FACE_FEATURE_LEN)
////#define FACE_FEATURE_Q 15
////#define MATCH_THRESHOLD  0.71
////#define MATCH_DIV2_SEARCH_MAX 20
////#define MATCH_MAX_VALID_NUM 10

#define ENABLE_805 


enum _USB_CHANNEL{
	USB_IN,
	USB_OUT,
	USB_NUM
};

enum _CAMERA_IMG{
	DEPTH_IMG,
	IR_IMG,
	RGB_IMG,
//	SGl_DEPTH_IMG,
//	SGL_IR_IMG,
//	SGL_RGB_IMG,
	IMG_NUM
};

typedef struct _image_t{
	uint32_t id;
	uint8_t type;
	uint8_t pixel_size;
	uint32_t len; // c * h * w
	uint16_t channel; 
	uint16_t height; 
	uint16_t width;
	uint32_t size; // c * h * w * pixel_size
	uint8_t *buf; 
}image_t;

typedef struct _UsbTsmPrivate{	
	LinkQueue queue[USB_NUM];
	aos_mutex_t mutex[USB_NUM];
	aos_sem_t sem[USB_NUM];
	
	uint32_t usb_cmd;
	image_t image[IMG_NUM];
	unsigned char *buf;

	resp_pack_head_t *not_ready;
	resp_pack_head_t *success;
	resp_pack_head_t *failed; 
	resp_pack_head_t *data_packet;
	int data_packet_size;
	int get_image;
	int aie_cal_flag;

	int ai_task_ctl;
}UsbTsmPrivate_t;

UsbTsmPrivate_t UsbTsmPrivate;
resp_pack_head_t *current_value;

extern struct _IMAGE_SOURCE    g_str_image_source;

#define CMD_HANDSHAKE_RSP 0x30
#define CMD_BIN_INFO_RSP  0x31
#define CMD_TRANSPORT_RSP 0x32
#define CRC_CHECK_BIT_LEN 0x20 /* 32 bit for crc32 */

uint32_t bin_total_len;
uint32_t current_bin_len;
uint32_t bin_len;
uint32_t add_id;
uint8_t *p_img_store = NULL;
uint32_t burn_addr = 0;
uint8_t sys_reset_flag = 0;

uint8_t usb_device_rsp_buff[512] = { 'T', 'S', 'M' };
uint8_t *bin_buf=NULL;
int32_t get_uint32_data(uint8_t* buff, uint32_t* data)
{
    *data = ((buff[0] << 24) + (buff[1] << 16) + (buff[2] << 8) + (buff[3] << 0));
    return 0;
}

int32_t set_uint32_data(uint8_t* buff, uint32_t data)
{
    buff[0] = (0xff) & (data >> 24);
    buff[1] = (0xff) & (data >> 16);
    buff[2] = (0xff) & (data >> 8);
    buff[3] = (0xff) & (data >> 0);
	return 0;
}

void usb_device_rsp_header_packeted(uint8_t cmd, uint8_t result,
    uint32_t id, uint32_t data_len)
{
    resp_pack_head_t* rsp_buff = (resp_pack_head_t*)usb_device_rsp_buff;
    memset(usb_device_rsp_buff, 0, sizeof(resp_pack_head_t));

    rsp_buff->head[0] = 'T';
    rsp_buff->head[1] = 'S';
    rsp_buff->head[2] = 'M';
    rsp_buff->cmd = cmd;
    rsp_buff->result = result;

    set_uint32_data(rsp_buff->id, id);
    set_uint32_data(rsp_buff->len, data_len);

    usb_trans_log("rsp header\r\n");
    for (uint32_t i = 0; i < sizeof(resp_pack_head_t); i++) {
        usb_trans_log("%02x ", usb_device_rsp_buff[i]);
    }
    usb_trans_log("\r\n");
}

void usb_device_rsp_set_crc(uint32_t data_len, uint32_t crc)
{
    resp_pack_head_t* rsp_buff = (resp_pack_head_t*)usb_device_rsp_buff;

    set_uint32_data(rsp_buff->dat, data_len);
}

#include "drv_wdt.h"
void sys_reboot()
{
    uint32_t irq_flag = 0;

    irq_flag = csi_irq_save();

    wdt_handle_t wdt_handle;
    wdt_handle = csi_wdt_initialize(0, NULL);
    csi_wdt_start(wdt_handle);
    csi_wdt_set_timeout(wdt_handle, 0);
    /* waiting for reboot */
    while (1)
        ;

    csi_irq_restore(irq_flag);
}

#include "drv_spiflash.h"

#define IMG_BURN_TEST_DATA_LEN (1 * 1024 * 1024)
#define MG_BURN_TEST_ADDR      (0xc0000000)
uint8_t* qspi_read_data = (uint8_t*)0x2000000;    /* 32M */
uint8_t* qspi_write_data = (uint8_t*)0x3000000;   /* 48M */

void spiflash_erase(int operate_addr, int operate_len)
{
    spiflash_info_t *info = NULL;
    uint32_t index;
    uint32_t erase_fail = 0;
    uint8_t data[256];
    uint32_t ret;
    uint32_t i;
    uint32_t j;
    uint32_t k;
    uint32_t l;
    uint32_t sector_num;
    spiflash_status_t status;
    spiflash_handle_t spiflash = NULL;
    index = 1;

    for (i = 0; i < index; i++) {
        spiflash = csi_spiflash_initialize(i, NULL);

        if (spiflash == NULL) {
            printf("spiflash initialize failed!\n");
            return;
        }

        info = csi_spiflash_get_info(spiflash);

        if (info == NULL) {
            printf("spiflash get info failed!\n");
            csi_spiflash_uninitialize(spiflash);
            return;
        }

        // sector_num = (operate_addr - CSKY_QSPIMEM_BASE + operate_len + info->sector_size - 1) / info->sector_size;
        sector_num = (operate_len + info->sector_size - 1) / info->sector_size;

        for (j = 0; j < sector_num; j++) {
            ret = csi_spiflash_erase_sector(spiflash, operate_addr & ~(info->sector_size - 1));

            if (ret != 0) {
                printf("the ret is %x the addr is %x \r\n", ret, operate_addr);
            }

            while (1) {
                status = csi_spiflash_get_status(spiflash);

                if (status.busy == 0) {
                    break;
                }
            }
            for (k = 0; k < info->sector_size / sizeof(data); k++) {
                ret = csi_spiflash_read(spiflash, operate_addr & ~(info->sector_size - 1), data, sizeof(data));
                // if (ret != 0)
                // {
                //     printf("%d:csi_spiflash_read ret:%08x\r\n", k, ret);
                // }

                for (l = 0; l < sizeof(data); l++) {
                    if (data[l] != info->erased_value) {
                        break;
                    }
                }

                if (l != sizeof(data)) {
                    erase_fail = 1;
                    printf("the index %u spiflash %x sector is erased failed\r\n", i, j);
                    break;
                }

                operate_addr = operate_addr + sizeof(data);
                operate_len -= sizeof(data);
            }

            if (erase_fail) {
                break;
            }
        }

        if (erase_fail) {
            printf("the index %u spiflash is erased failed\r\n", i);
            return;
        } else {
            printf("the index %u spiflash is erased pass\r\n", i);
        }
        csi_spiflash_uninitialize(spiflash);
    }
}

void spiflash_program(uint8_t *src_data, int operate_addr, int operate_len)
{
    uint8_t *source_data = src_data;;
    uint32_t i;
    uint32_t sector_num;
    spiflash_handle_t spiflash = NULL;

    spiflash = csi_spiflash_initialize(0, NULL);
    sector_num = (operate_len + 512) / 512;

    for (i = 0; i < sector_num; i++) {
        csi_spiflash_program(spiflash, operate_addr, source_data, 512);

        operate_addr += 512;
        source_data += 512;
    }
    csi_spiflash_uninitialize(spiflash);
}

int img_program(uint8_t* img_store, uint32_t buring_addr, uint32_t img_len)
{
    if ((buring_addr < 0xc0000000)
        || (buring_addr > (0xc0000000 + 0x1000000 - 1))
        || (img_len == 0)
        || (img_len > 0x10000000)) {
        return -1;
    }
		
//	hal_qflash_adapt_mutex_lock();	
//    spiflash_erase(buring_addr, img_len);
//    spiflash_program(img_store, buring_addr, img_len);
//	hal_qflash_adapt_mutex_unlock();
	return qspi_flash_write(buring_addr, img_store, img_len);

}	

static uint32_t Table2[256];
static uint64_t reflect(uint64_t ref, uint8_t ch)
{
	int i;
	uint64_t value = 0;
	for (i = 1; i < (ch + 1); i++) {
		if (ref & 1) {
			value |= 1 << (ch - i);
		}
		ref >>= 1;
	}
	return value;
}

static void gen_normal_table(uint32_t *table)
{
	uint32_t gx = 0x04c11db7;
	uint32_t temp;
	int i, j;
	for (i = 0; i <= 0xFF; i++) {
		temp = reflect(i, 8);
		table[i] = temp << 24;
		for (j = 0; j < 8; j++) {
			unsigned long int t1, t2;
			unsigned long int flag = table[i] & 0x80000000;
			t1 = (table[i] << 1);
			if (flag == 0) {
				t2 = 0;
			}
			else {
				t2 = gx;
			}
			table[i] = t1 ^ t2;
		}
		table[i] = reflect(table[i], 32);
	}
}

static uint32_t reverse_table_crc(uint8_t *data, int32_t len, uint32_t *table)
{
	uint32_t crc = 0xffffffff;
	uint8_t *p = data;
	int i;
	for (i = 0; i < len; i++) {
		crc = table[(crc ^ (*(p + i))) & 0xff] ^ (crc >> 8);
	}
	return  ~crc;
}

static uint32_t crc32_bit(uint8_t *ptr, uint32_t len)
{
	uint32_t crc;
	static uint8_t init_table = 0;
	if (init_table == 0) {
		init_table = 1;
		gen_normal_table(Table2);
	}
	crc = reverse_table_crc(ptr, len, Table2);
	return crc;
}



void ftempl_func(int port, int notification)
{
	switch(notification){
	case SUD_FTEMPL_NOTIFY_INTDATAREADY:
		printf("rece int\n");
		break;
	case SUD_FTEMPL_NOTIFY_BULKDATAREADY:
		aos_mutex_lock(&UsbTsmPrivate.mutex[USB_IN], AOS_WAIT_FOREVER);
		LinkNode *node = (LinkNode *)GetEmptyNode(&UsbTsmPrivate.queue[USB_IN]);
		if(node==NULL){
			printf("queue full \n");
		}else{
			sud_FTemplGetBulkData(USB_PORT, (unsigned char*)node->buf, SUD_FTEMPL_BULKIN_PKTSIZE);
			QueueSendFull(&UsbTsmPrivate.queue[USB_IN], node, SUD_FTEMPL_BULKIN_PKTSIZE);
			
			//static int usb_re_count = 0;
			//printf("usb_re packet :%d \n", usb_re_count++);
		}
		aos_mutex_unlock(&UsbTsmPrivate.mutex[USB_IN]);
		break;
	default: 
		break;
	}
	
}

static int usb_send_pack(char *log_s, uint32_t pack_size, uint8_t *p_pack)
{
	int pack_cnt = pack_size/SUD_FTEMPL_BULKOUT_PKTSIZE;
	int offset = 0;
	while(pack_cnt--)
	{
		if(sud_FTemplSendBulk(USB_PORT, p_pack + offset, SUD_FTEMPL_BULKOUT_PKTSIZE) == 0)
		{
			offset += SUD_FTEMPL_BULKOUT_PKTSIZE;
			continue;
		}
		printf("usb port%d send error!\r\n", USB_PORT);
		return -1;
	}
	if(log_s != NULL)
		printf("%s send success\n", log_s);
	return 0;
}

#if 0
static int usb_send_pack_failed()
{
	if(sud_FTemplSendBulk(USB_PORT, (u8 *)UsbTsmPrivate.failed, SUD_FTEMPL_BULKOUT_PKTSIZE) == 0)
	{
		printf("send to usb failed! \r\n");
		return 0;
	}	
	printf("usb port%d send error!\r\n", USB_PORT);
	return -1;
}
#endif

static int usb_send_pack_success()
{
	if(sud_FTemplSendBulk(USB_PORT, (u8 *)UsbTsmPrivate.success, SUD_FTEMPL_BULKOUT_PKTSIZE) == 0)
	{
		printf("send to usb success ! \r\n");
		return 0;
	}	
	printf("usb port%d send error!\r\n", USB_PORT);
	return -1;
}


static int usb_send_pack_not_ready()
{
	if(sud_FTemplSendBulk(USB_PORT, (u8 *)UsbTsmPrivate.not_ready, SUD_FTEMPL_BULKOUT_PKTSIZE) == 0)
	{
		//printf("send to usb success -not ready! \r\n");
		return 0;
	}	
	printf("usb port%d send error!\r\n", USB_PORT);
	return -1;
}

static int usb_send_image(char *log_s, image_t *image)
{
	pack_resp_head(CMD_GET_RESULT, RESULT_SUCCESS, 0, 6+image->size, (uint8_t *)UsbTsmPrivate.data_packet);
	uint32_t pack_size = ceil((float)(RESP_IMAGE_HEAD_SIZE+image->size)/SUD_FTEMPL_BULKOUT_PKTSIZE) * SUD_FTEMPL_BULKOUT_PKTSIZE;
	uint8_t *data = (uint8_t *)&UsbTsmPrivate.data_packet->dat[0];
	store_le_uint16(&data[0], image->channel);
	store_le_uint16(&data[2], image->height);
	store_le_uint16(&data[4], image->width);
	
	int remain_len = SUD_FTEMPL_BULKOUT_PKTSIZE - RESP_IMAGE_HEAD_SIZE;

	memcpy(&data[6], image->buf, remain_len);
#if defined(PARA_TRAV) 	
	if (0 == memcmp(log_s, "rgb image", sizeof("rgb image"))) {		
		pack_trav_para_list(data);
#if defined(PARA_TRAV_DEBUG) 
		for(int i = 0; i < 5; i++) 
			printf("data[%d] = %x\n", i, data[i]);
#endif		
	}
#endif		
	if(sud_FTemplSendBulk(USB_PORT, (u8 *)UsbTsmPrivate.data_packet, SUD_FTEMPL_BULKOUT_PKTSIZE) != 0)
	{
		printf("usb port%d send error!\r\n", USB_PORT);
		return -1;
	}	
	return usb_send_pack(log_s, pack_size-SUD_FTEMPL_BULKOUT_PKTSIZE, image->buf + remain_len);
}

static int usb_send_one_depth_image()
{
	int depth_image_addr = vi_get_snap_3in1_depth_adr();
	g_depth_image_status=1;
	if(g_depth_image_status)
	{
		g_depth_image_status = false;
		if(sud_FTemplSendBulk(USB_PORT, (uint8_t *)depth_image_addr, DEPTH_IMAGE_SIZE) != 0)
		{
			printf("usb port%d send error!\r\n", USB_PORT);
			return -1;
		}
		printf("depth image send success\n");			
	}
	else
	{
		printf("depth picture not ready!\r\n");
		return -1;		
	}

	return 0;
}

static char usb_uvc_set_image_source(uint8_t *data)
{
	g_str_image_source.type = *data;
	
	#if 1
	if(g_str_image_source.type == RGB)
	{
		printf("source rgb\r\n");
		vi_set_snap_mode(SNAPSHOT_MODE_DONE);
		vi_set_snap_mode(SNAPSHOT_MODE_UVC_RGB_720P_YUV);
	}
	else if(g_str_image_source.type == IR)
	{
		printf("source ir\r\n");
		vi_set_snap_mode(SNAPSHOT_MODE_DONE);
		vi_set_snap_mode(SNAPSHOT_MODE_UVC_IR_720P_YUV);
	}
	else if(g_str_image_source.type == DEPTH)
	{
		printf("source dep\r\n");
		vi_set_snap_mode(SNAPSHOT_MODE_DONE);
		vi_set_snap_mode(SNAPSHOT_MODE_UVC_DEPTH_640X360_RAW16);
		//vi_isp1_switch2spk2_mode();
	}	
	else
	{
		printf("g_str_image_source.type = %02X",g_str_image_source.type);
	}
	#endif
	return 0;
}

static int usb_send_nblock()
{
	//UsbTsmPrivate.aie_cal_flag = true;
	
	if(CMD_SET_IMG_PARAM == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}

#if defined(TX510_EG_360_FACTORY_MODE)

	if(CMD_TEST_ELECTRIC_OFF == UsbTsmPrivate.usb_cmd) //——>0x01
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_ELECTRIC_ON == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_SN == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_LCD == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_CAMERA == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_LED == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_BT_WIFI == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_NETWORK == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_DATA == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_SENSOR == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_MIC_SPEAKER == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_PREVENT == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_USB_OTG == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_WG == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_WARNING == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_CONNECT == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_UART == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
	if(CMD_TEST_GPIO == UsbTsmPrivate.usb_cmd)
	{
		//printf("success\n");
		if(0!=usb_send_pack_success())
			return -1;
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		UsbTsmPrivate.ai_task_ctl = false;
		return 0;
	}
#endif 
	
	if(UsbTsmPrivate.aie_cal_flag)
	{
		//vi_set_snap_mode(SNAPSHOT_MODE_DONE);
		if((CMD_GET_ALL_IMG == UsbTsmPrivate.usb_cmd)  || (CMD_GET_ALL_IMG_SPK == UsbTsmPrivate.usb_cmd))
		{
			//printf("all image\n");
			if(0!=usb_send_image("depth image", &UsbTsmPrivate.image[DEPTH_IMG])){
				return -1;
			}

			if(0!=usb_send_image("ir image", &UsbTsmPrivate.image[IR_IMG])){
				return -1;
			}

			if(0!=usb_send_image("rgb image", &UsbTsmPrivate.image[RGB_IMG])){
				return -1;
			}
		}
		else if(CMD_GET_DEPTH_IMG == UsbTsmPrivate.usb_cmd)
		{
			//printf("depth image\n");
			if(0!=usb_send_image("depth image", &UsbTsmPrivate.image[DEPTH_IMG]))
			{
				return -1;
			}
		}
		else if(CMD_GET_IR_IMG == UsbTsmPrivate.usb_cmd)
		{
			//printf("ir image\n");
			if(0!=usb_send_image("ir image", &UsbTsmPrivate.image[IR_IMG]))
			{
				return -1;
			}
		}
		else if(CMD_GET_RGB_IMG == UsbTsmPrivate.usb_cmd)
		{
			//printf("rgb image\n");
			if(0!=usb_send_image("rgb image", &UsbTsmPrivate.image[RGB_IMG]))
			{
				return -1;
			}
		}
		vi_set_snap_mode(SNAPSHOT_MODE_DONE);
		UsbTsmPrivate.get_image = false;
		UsbTsmPrivate.aie_cal_flag = false;
		g_snapshot_done=0;
		return 0;
	}
	else
	{	
		static int count_not_ready =0;
		count_not_ready++;
		if(count_not_ready == 30)
		{
			printf("not_ready\n");
			count_not_ready = 0;
		}
		return usb_send_pack_not_ready();
	}
	return 0;
}

static int parse_set_img_info(req_pack_head_t *packet)
{
	uint32_t offset = 0;
	image_t *image_tmp = &UsbTsmPrivate.image[IR_IMG];
	image_tmp->type = packet->dat[offset+0];
	image_tmp->pixel_size = packet->dat[offset+1];
	dump_le_uint16(&packet->dat[offset+2], &image_tmp->channel);
	dump_le_uint16(&packet->dat[offset+4], &image_tmp->height);
	dump_le_uint16(&packet->dat[offset+6], &image_tmp->width);
	//image_tmp->size = image_tmp->channel * image_tmp->height * image_tmp->width * image_tmp->pixel_size;

	offset += 8;
	image_tmp = &UsbTsmPrivate.image[RGB_IMG];
	image_tmp->type = packet->dat[offset+0];
	image_tmp->pixel_size = packet->dat[offset+1];
	dump_le_uint16(&packet->dat[offset+2], &image_tmp->channel);
	dump_le_uint16(&packet->dat[offset+4], &image_tmp->height);
	dump_le_uint16(&packet->dat[offset+6], &image_tmp->width);
	//image_tmp->size = image_tmp->channel * image_tmp->height * image_tmp->width * image_tmp->pixel_size;

	offset += 8;
	image_tmp = &UsbTsmPrivate.image[DEPTH_IMG];
	image_tmp->type = packet->dat[offset+0];
	image_tmp->pixel_size = packet->dat[offset+1];
	dump_le_uint16(&packet->dat[offset+2], &image_tmp->channel);
	dump_le_uint16(&packet->dat[offset+4], &image_tmp->height);
	dump_le_uint16(&packet->dat[offset+6], &image_tmp->width);
	//image_tmp->size = image_tmp->channel * image_tmp->height * image_tmp->width * image_tmp->pixel_size;
	
#if 1
	printf("ir_img(%d,%d,%d,%d);\n", UsbTsmPrivate.image[IR_IMG].pixel_size,\
										UsbTsmPrivate.image[IR_IMG].channel,\
										UsbTsmPrivate.image[IR_IMG].height,\
										UsbTsmPrivate.image[IR_IMG].width);
	
	printf("rgb_img(%d,%d,%d,%d);\n", UsbTsmPrivate.image[RGB_IMG].pixel_size,\
										UsbTsmPrivate.image[RGB_IMG].channel,\
										UsbTsmPrivate.image[RGB_IMG].height,\
										UsbTsmPrivate.image[RGB_IMG].width);
	
	printf("dep_img(%d,%d,%d,%d);\n", UsbTsmPrivate.image[DEPTH_IMG].pixel_size,\
										UsbTsmPrivate.image[DEPTH_IMG].channel,\
										UsbTsmPrivate.image[DEPTH_IMG].height,\
										UsbTsmPrivate.image[DEPTH_IMG].width);
#endif
	return 0;
}



extern uint32_t g_usb_image_type;
product_info_s * info_w;
static int decode_protocal() //pc_protocal
{
	//printf("hello world\n");
 	aos_mutex_lock(&UsbTsmPrivate.mutex[USB_IN], AOS_WAIT_FOREVER);
 	LinkNode *node = (LinkNode *)GetFullNode(&UsbTsmPrivate.queue[USB_IN]);
 	if(node == NULL)
 	{
 		aos_mutex_unlock(&UsbTsmPrivate.mutex[USB_IN]);
 		return 0;
 	}
	
	req_pack_head_t *req_packet = (req_pack_head_t *)node->buf;	
	if(req_packet->head[0] != 'T' \
		|| req_packet->head[1] != 'S' \
		|| req_packet->head[2] != 'M')
	{
		QueueSendEmpty(&UsbTsmPrivate.queue[USB_IN], node, 0);
		aos_mutex_unlock(&UsbTsmPrivate.mutex[USB_IN]);
		printf("USB PORT PACKET NOT SUPPORT!  %c %c %c\n", 
			req_packet->head[0],req_packet->head[1],req_packet->head[2]);
		return 0;
	}
	uint8_t cmd = GET_REQ_CMD(req_packet);
	printf("cmd is %x\n", cmd);
	if(cmd == -1)
		return 0;
	//printf("decode: cmd:0x%x\n", cmd);

#if defined(TX510_EG_360_FACTORY_MODE)
	uint8_t second_command = 0;
	if(cmd == CMD_SET_IMG_PARAM)
	{
		printf("CMD_SET_IMG_PARAM \n");
		parse_set_img_info((req_pack_head_t *)node->buf);
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_ELECTRIC_OFF) //关机电流——>0x01
	{
		printf("CMD_TEST_DOWNA\n");
		
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_ELECTRIC_ON) //开机电流——>0x02
	{
		printf("CMD_TEST_RUNA\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_SN) //写SN——>0x03
	{
		
		info_w  = &((req_pack_head_t *)node->buf)->len[0];
		//test_flash();
		printf("CMD_TEST_SN\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_LCD) //测试LCD——>0x04
	{
		printf("CMD_TEST_LCD\n");
		//test_lcd();
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_CAMERA) //测试摄像头——>0x05
	{
		second_command = ((req_pack_head_t *)node->buf)->len[0];
		printf("second command is %x\n",second_command);
		printf("CMD_TEST_CAMERA\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_LED) //测试LED——>0x06
	{
		second_command = ((req_pack_head_t *)node->buf)->len[0];
		printf("second command is %x\n",second_command);
		printf("CMD_TEST_LED\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_BT_WIFI) //测试WIFI/BT——>0x07
	{
		printf("CMD_TEST_BT/WIFI\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_NETWORK) //测试网络——>0x08
	{
		printf("CMD_TEST_NETWORK\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_DATA) //测试数据——>0x09
	{
		printf("CMD_TEST_DATA\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_SENSOR) //测试sensor——>0x0A
	{
		printf("CMD_TEST_SENSOR\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_MIC_SPEAKER) //测试MIC——>0x0B
	{
		printf("CMD_TEST_MIC\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_PREVENT) //测试防拆——>0x0C
	{
		printf("CMD_TEST_PREVENT\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_USB_OTG) //测试USB OTG——>0x0D
	{
		printf("CMD_TEST_USB\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_WG) //测试WG ——>0x0E
	{
		printf("CMD_TEST_WG\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_WARNING) //测试报警——>0x0F
	{
		printf("CMD_TEST_WARNING\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_CONNECT) //测试干接点——>0x10
	{
		printf("CMD_TEST_CONNECT\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_UART) //测试串口——>0x11
	{
		printf("CMD_TEST_UART\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_TEST_GPIO) //测试GPIO——>0x12
	{
		printf("CMD_TEST_GPIO\n");
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	/*else if(cmd == CMD_GET_ALL_IMG)
	{
		printf("CMD_GET_ALL_IMG \n");
		if(UsbTsmPrivate.get_image==false)
		{
			UsbTsmPrivate.image[IR_IMG].buf = (uint8_t *)vi_get_snap_3in1_ir_adr();
			UsbTsmPrivate.image[RGB_IMG].buf = (uint8_t *)vi_get_snap_3in1_rgb_adr();
			UsbTsmPrivate.image[DEPTH_IMG].buf = (uint8_t *)vi_get_snap_3in1_depth_adr();
			UsbTsmPrivate.image[IR_IMG].size = (uint32_t)IR_IMAGE_SIZE;
			UsbTsmPrivate.image[RGB_IMG].size = (uint32_t)RGB_IMAGE_SIZE;
			UsbTsmPrivate.image[DEPTH_IMG].size = (uint32_t)DEPTH_IMAGE_SIZE;
			vi_set_snap_mode(SNAPSHOT_MODE_3IN1);
			SET_USB_CTRL_SATUE(cmd);
		}else{
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}
	else if(cmd == CMD_GET_ALL_IMG_SPK)
	{
		printf("CMD_GET_ALL_IMG_SPK \n");
		if(UsbTsmPrivate.get_image==false)
		{	
			UsbTsmPrivate.image[IR_IMG].buf = (uint8_t *)vi_get_snap_3in1_ir_adr();
			UsbTsmPrivate.image[RGB_IMG].buf = (uint8_t *)vi_get_snap_3in1_rgb_adr();
			UsbTsmPrivate.image[DEPTH_IMG].buf = (uint8_t *)vi_get_snap_3in1_spk_adr();
			UsbTsmPrivate.image[IR_IMG].size = (uint32_t)IR_IMAGE_SIZE;
			UsbTsmPrivate.image[RGB_IMG].size = (uint32_t)RGB_IMAGE_SIZE;
			UsbTsmPrivate.image[DEPTH_IMG].size = (uint32_t)IR_IMAGE_SIZE;
			vi_set_snap_mode(SNAPSHOT_MODE_3IN1);
			SET_USB_CTRL_SATUE(cmd);
		}else{
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}
	else if(cmd == CMD_GET_IR_IMG)
	{
		printf("CMD_GET_IR_IMG \n");
		if(UsbTsmPrivate.get_image==false)
		{	
			UsbTsmPrivate.image[IR_IMG].buf = (uint8_t *)vi_get_snap_ir_720P_raw10_adr();
			UsbTsmPrivate.image[IR_IMG].size = (uint32_t)SGL_IR_IMAGE_SIZE;
			vi_set_snap_mode(SNAPSHOT_MODE_IR_720P_RAW10);
			SET_USB_CTRL_SATUE(cmd);
			
		}else{
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}
	else if(cmd == CMD_GET_RGB_IMG)
	{
		printf("CMD_GET_RGB_IMG \n");
		if(UsbTsmPrivate.get_image==false)
		{
			UsbTsmPrivate.image[RGB_IMG].buf = (uint8_t *)vi_get_snap_3in1_rgb_adr();
			UsbTsmPrivate.image[RGB_IMG].size = (uint32_t)SGL_RGB_IMAGE_SIZE;
			vi_set_snap_mode(SNAPSHOT_MODE_RGB_1080P_YUV);
			SET_USB_CTRL_SATUE(cmd);
		}else{
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}
 	else if(cmd == CMD_GET_DEPTH_IMG)
	{
		printf("CMD_GET_DEPTH_IMG \n");
		if(UsbTsmPrivate.get_image==false)
		{
			SET_USB_CTRL_SATUE(cmd);
			UsbTsmPrivate.image[DEPTH_IMG].buf = (uint8_t *)vi_get_snap_3in1_depth_adr();
			UsbTsmPrivate.image[DEPTH_IMG].size = (uint32_t)DEPTH_IMAGE_SIZE;
			vi_isp1_switch2spk2_mode();
		}else{
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}*/
 	else if(cmd == CMD_GET_RESULT)
 	{
		if(UsbTsmPrivate.get_image == true)
 		{
			int ret = usb_send_nblock();
			if(0!=ret)
			{
			}		
			//printf("UsbTsmPrivate.set_aie_flag0 = %d \n", UsbTsmPrivate.set_aie_flag);
 		}else{			
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
 	}
	else if(cmd == CMD_GET_DEPTH_PIC)
	{	
		//usb_image_type = USB_IMAGE_TYPE_DEPTH;
		printf("switch to depth mode \n");
		usb_send_one_depth_image();
	}
	else if(cmd == CMD_SET_IMA_SOURCE)
	{	
        usb_uvc_set_image_source(req_packet->dat);
	}
	else{
		printf("USB PORT%d CMD NOT SUPPORT!\n", USB_PORT);
	}

#else 
	if(cmd == CMD_SET_IMG_PARAM)
	{
		printf("CMD_SET_IMG_PARAM \n");
		parse_set_img_info((req_pack_head_t *)node->buf);
		SET_USB_CTRL_SATUE(cmd);
		UsbTsmPrivate.ai_task_ctl = false;
	}
	else if(cmd == CMD_GET_ALL_IMG)
	{
		printf("CMD_GET_ALL_IMG \n");
		if(UsbTsmPrivate.get_image==false)
		{
			UsbTsmPrivate.image[IR_IMG].buf = (uint8_t *)vi_get_snap_3in1_ir_adr();
			UsbTsmPrivate.image[RGB_IMG].buf = (uint8_t *)vi_get_snap_3in1_rgb_adr();
			UsbTsmPrivate.image[DEPTH_IMG].buf = (uint8_t *)vi_get_snap_3in1_depth_adr();
			UsbTsmPrivate.image[IR_IMG].size = (uint32_t)IR_IMAGE_SIZE;
			UsbTsmPrivate.image[RGB_IMG].size = (uint32_t)RGB_IMAGE_SIZE;
			UsbTsmPrivate.image[DEPTH_IMG].size = (uint32_t)DEPTH_IMAGE_SIZE;
			vi_set_snap_mode(SNAPSHOT_MODE_3IN1);
			SET_USB_CTRL_SATUE(cmd);
		}else{
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}
	else if(cmd == CMD_GET_ALL_IMG_SPK)
	{
		printf("CMD_GET_ALL_IMG_SPK \n");
		if(UsbTsmPrivate.get_image==false)
		{	
			UsbTsmPrivate.image[IR_IMG].buf = (uint8_t *)vi_get_snap_3in1_ir_adr();
			UsbTsmPrivate.image[RGB_IMG].buf = (uint8_t *)vi_get_snap_3in1_rgb_adr();
			UsbTsmPrivate.image[DEPTH_IMG].buf = (uint8_t *)vi_get_snap_3in1_spk_adr();
			UsbTsmPrivate.image[IR_IMG].size = (uint32_t)IR_IMAGE_SIZE;
			UsbTsmPrivate.image[RGB_IMG].size = (uint32_t)RGB_IMAGE_SIZE;
			UsbTsmPrivate.image[DEPTH_IMG].size = (uint32_t)IR_IMAGE_SIZE;
			vi_set_snap_mode(SNAPSHOT_MODE_3IN1);
			SET_USB_CTRL_SATUE(cmd);

			//printf("usb transmit done \n");
			if_vRgbStreamPauseResume(1);

		}else{
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}
	else if(cmd == CMD_GET_IR_IMG)
	{
		printf("CMD_GET_IR_IMG \n");
		if(UsbTsmPrivate.get_image==false)
		{	
			UsbTsmPrivate.image[IR_IMG].buf = (uint8_t *)vi_get_snap_ir_720P_raw10_adr();
			UsbTsmPrivate.image[IR_IMG].size = (uint32_t)SGL_IR_IMAGE_SIZE;
			vi_set_snap_mode(SNAPSHOT_MODE_IR_720P_RAW10);
			SET_USB_CTRL_SATUE(cmd);
			
		}else{
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}
	else if(cmd == CMD_GET_RGB_IMG)
	{
		printf("CMD_GET_RGB_IMG \n");
		if(UsbTsmPrivate.get_image==false)
		{
			UsbTsmPrivate.image[RGB_IMG].buf = (uint8_t *)vi_get_snap_3in1_rgb_adr();
			UsbTsmPrivate.image[RGB_IMG].size = (uint32_t)SGL_RGB_IMAGE_SIZE;
			vi_set_snap_mode(SNAPSHOT_MODE_RGB_1080P_YUV);
			SET_USB_CTRL_SATUE(cmd);
		}else{
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}
	else if(cmd == CMD_GET_DEPTH_IMG)
	{
		printf("CMD_GET_DEPTH_IMG \n");
		if(UsbTsmPrivate.get_image==false)
		{
			SET_USB_CTRL_SATUE(cmd);
			UsbTsmPrivate.image[DEPTH_IMG].buf = (uint8_t *)vi_get_snap_3in1_depth_adr();
			UsbTsmPrivate.image[DEPTH_IMG].size = (uint32_t)DEPTH_IMAGE_SIZE;
			vi_isp1_switch2spk2_mode();
		}else{
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}
	else if(cmd == CMD_GET_RESULT)
	{
		if(UsbTsmPrivate.get_image == true)
		{
			int ret = usb_send_nblock();
			if(0!=ret)
			{
			}		
			//printf("UsbTsmPrivate.set_aie_flag0 = %d \n", UsbTsmPrivate.set_aie_flag);
		}else{			
			printf("USB PORT%d CMD STATUS ERROR!\n", USB_PORT);
		}
	}
	else if(cmd == CMD_GET_DEPTH_PIC)
	{	
		//usb_image_type = USB_IMAGE_TYPE_DEPTH;
		printf("switch to depth mode \n");
		usb_send_one_depth_image();
	}
	else if(cmd == CMD_SET_IMA_SOURCE)
	{	
		usb_uvc_set_image_source(req_packet->dat);
	}
	else if (cmd == CMD_HANDSHAKE_REQ) {

        usb_device_rsp_header_packeted(CMD_HANDSHAKE_RSP, 0, 0, 0);
        uint32_t crc = crc32_bit(req_packet->dat, 0);
        usb_device_rsp_set_crc(0, crc);
        usb_trans_log("crc packet:0x%08x\r\n", crc);
    } else if (cmd == CMD_BIN_INFO_REQ) {
        get_uint32_data(&req_packet->dat[4], &bin_total_len);
        get_uint32_data(req_packet->dat, &burn_addr);

        current_bin_len = 0; //clear current len
        p_img_store= (uint8_t*)bin_buf;

        usb_trans_log("bin_total_len = %u\r\n", bin_total_len);
        usb_trans_log("burn_addr = %u\r\n", burn_addr);

        if (bin_total_len == 0) {
            usb_device_rsp_header_packeted(CMD_BIN_INFO_RSP, 1, 0, 0); /* result */
        } else {
            usb_device_rsp_header_packeted(CMD_BIN_INFO_RSP, 0, 0, 0); /* result */
        }

        uint32_t crc = crc32_bit(req_packet->dat, 0);
        usb_device_rsp_set_crc(0, crc);
        usb_trans_log("crc packet:0x%08x\r\n", crc);
    } 
	else if (cmd == CMD_TRANSPORT_REQ) {
        bin_len = GET_REQ_LEN(req_packet);
        current_bin_len += bin_len;
        usb_trans_log("bin_len = %d,current_len = %d,bin_total_len = %d\r\n",
            bin_len, current_bin_len, bin_total_len);

        // static uint32_t pkt_counter = 0;
        // usb_trans_log("pkt_counter:%d\r\n", pkt_counter++);

        if (current_bin_len <= bin_total_len) {
            /* store img in ddr */
            memcpy(p_img_store, req_packet->dat, bin_len);
            p_img_store += bin_len;
            usb_device_rsp_header_packeted(CMD_TRANSPORT_RSP, 0, 0, 0); /* result */

            uint32_t crc = crc32_bit(req_packet->dat, 0);
            usb_device_rsp_set_crc(0, crc);
            usb_trans_log("crc packet:0x%08x\r\n", crc);
        } else {
            printf("current_bin_len error\r\n");
            goto opt_error;
        }
    } 
	else if (cmd == CMD_FLASH_REQ) {
		burn_addr |= 0xc0000000;
		/* burning */
        img_program((uint8_t*)bin_buf, burn_addr, bin_total_len);

        usb_device_rsp_header_packeted(CMD_FLASH_RES, 0, 0, 0);

        uint32_t crc = crc32_bit(req_packet->dat, 0);
        usb_device_rsp_set_crc(0, crc);
        usb_trans_log("crc packet:0x%08x\r\n", crc);
    } else if (cmd == CMD_RESET_REQ) {
        sys_reset_flag = 1; /* system will reset after send response */
        usb_device_rsp_header_packeted(CMD_RESET_RES, 0, 0, 0);

        uint32_t crc = crc32_bit(req_packet->dat, 0);
        usb_device_rsp_set_crc(0, crc);
        usb_trans_log("crc packet:0x%08x\r\n", crc);
    } else {
        printf("USB PORT%d CMD NOT SUPPORT!\n", USB_PORT);
		goto opt_error;
    }

	if(cmd == CMD_HANDSHAKE_REQ
		|| cmd == CMD_BIN_INFO_REQ
		|| cmd == CMD_TRANSPORT_REQ
		|| cmd == CMD_FLASH_REQ
		|| cmd == CMD_RESET_REQ)
	{
//		usb_trans_log("sendbuffer:\r\n");
//		for (uint32_t i = 0; i < 32; i++) {
//			usb_trans_log("%02x ", usb_device_rsp_buff[i]);
//		}
//		usb_trans_log("\r\n");
		/* send usb packet */
		if (sud_FTemplSendBulk(USB_PORT, usb_device_rsp_buff, SUD_FTEMPL_BULKOUT_PKTSIZE) == 0) {
			usb_trans_log("send to usb success! \r\n");
		} else {
			printf("usb port%d send error!\r\n", USB_PORT);
		}
	}


#endif 
opt_error:
	QueueSendEmpty(&UsbTsmPrivate.queue[USB_IN], node, 0);
 	aos_mutex_unlock(&UsbTsmPrivate.mutex[USB_IN]);
	/* reset system */
    if (1 == sys_reset_flag) {
        sys_reset_flag = 0;
        printf("system reset\r\n");
        sys_reboot();
    }
	return 0;
}

static void AI_task(void *paras)
{
	const char *task_name = aos_task_name();
	printf("task name: %s start!\r\n", task_name);
	//UsbTsmPrivate.set_aie_flag = true;
	//int ret = -1;
	while(1)
	{
		if(UsbTsmPrivate.ai_task_ctl)
		{
			if(vi_get_snap_status())
			{
				printf("ai-task: get status done \n");
				//vi_set_snap_mode(SNAPSHOT_MODE_DONE);
				UsbTsmPrivate.aie_cal_flag = true;
				UsbTsmPrivate.ai_task_ctl = false;
			}	
		}
        aos_msleep(1);
    }
}

static int usbtsm_init(void)
{
	printf("%s %d \n",__func__,__LINE__);

	memset(&UsbTsmPrivate, 0, sizeof(UsbTsmPrivate_t));
	int ret = -1;
	if(QueueInit(&(UsbTsmPrivate.queue[USB_IN]), SUD_FTEMPL_BULKIN_QUEUE_SIZE, SUD_FTEMPL_BULKIN_PKTSIZE) == -1)
	{
		printf("QueueInit malloc error !!!\r\n");
		return -1;
	}
	printf("%s %d \n",__func__,__LINE__);
	if(QueueInit(&(UsbTsmPrivate.queue[USB_OUT]), SUD_FTEMPL_BULKOUT_QUEUE_SIZE, SUD_FTEMPL_BULKOUT_PKTSIZE) == -1)
	{
		printf("QueueInit malloc error !!!\r\n");
		return -1;
	}
	printf("%s %d \n",__func__,__LINE__);
	ret = aos_mutex_new(&UsbTsmPrivate.mutex[USB_IN]);
    if (ret != 0)
	{
		printf("mutex new error !!!\r\n");
        return ret;
    }
	printf("%s %d \n",__func__,__LINE__);
	ret = aos_mutex_new(&UsbTsmPrivate.mutex[USB_OUT]);
    if (ret != 0)
	{
		printf("mutex new error !!!\r\n");
        return ret;
    }
	ret = aos_sem_new(&UsbTsmPrivate.sem[USB_IN], 0);
	if (ret != 0) {
		printf("sem create failed\r\n");
	}
	ret = aos_sem_new(&UsbTsmPrivate.sem[USB_OUT], 0);
	if (ret != 0) {
		printf("sem create failed\r\n");
	}
	printf("%s %d \n",__func__,__LINE__);
	UsbTsmPrivate.buf = aos_calloc(1, 2*1024*1024);
	if(UsbTsmPrivate.buf == NULL){
		printf("buf malloc error !!!\r\n");
		return -1;
	}
	
	printf("%s %d \n",__func__,__LINE__);
//	UsbTsmPrivate.image_data.buf = (uint8_t *) (((unsigned int)aos_malloc(2*1024*1024+16)+16)/16*16);
//	if(UsbTsmPrivate.image_data.buf == NULL){
//		printf("buf malloc error !!!\r\n");
//		return -1;
//	}
//	if((((unsigned int)UsbTsmPrivate.image_data.buf) % 0x10) != 0){
//		printf("imagebug addr error !!!\r\n");
//		return -1;
//	}
//	printf("image_data.buf_addr=0x%x\r\n",(unsigned int)UsbTsmPrivate.image_data.buf);

	UsbTsmPrivate.not_ready = aos_malloc(SUD_FTEMPL_BULKOUT_PKTSIZE);
	if(UsbTsmPrivate.not_ready ==NULL){
		printf("malloc error !!!\r\n");
		return -1;
	}
	UsbTsmPrivate.success = aos_malloc(SUD_FTEMPL_BULKOUT_PKTSIZE);
	if(UsbTsmPrivate.success ==NULL){
		printf("malloc error !!!\r\n");
		return -1;
	}
	UsbTsmPrivate.failed = aos_malloc(SUD_FTEMPL_BULKOUT_PKTSIZE);
	if(UsbTsmPrivate.failed ==NULL){
		printf("malloc error !!!\r\n");
		return -1;
	}
	pack_resp_head(CMD_GET_RESULT,RESULT_NOT_READY, 0, 0, (uint8_t *)UsbTsmPrivate.not_ready);
	pack_resp_head(CMD_GET_RESULT,RESULT_SUCCESS, 0, 0, (uint8_t *)UsbTsmPrivate.success);
	pack_resp_head(CMD_GET_RESULT,RESULT_SUCCESS, 0, 0, (uint8_t *)current_value);
	pack_resp_head(CMD_GET_RESULT,RESULT_FAILED, 0, 0, (uint8_t *)UsbTsmPrivate.failed);
	///////////////SGL_RGB_IMAGE size is max
	UsbTsmPrivate.data_packet_size = ceil((float)(RESP_IMAGE_HEAD_SIZE+SGL_RGB_IMAGE_SIZE)/SUD_FTEMPL_BULKOUT_PKTSIZE) * SUD_FTEMPL_BULKOUT_PKTSIZE;
	UsbTsmPrivate.data_packet = aos_malloc(UsbTsmPrivate.data_packet_size);
	if(UsbTsmPrivate.data_packet == NULL){
		printf("malloc error !!!\r\n");
		return -1;
	}
	pack_resp_head(CMD_GET_RESULT,RESULT_SUCCESS, 0, 6+SGL_RGB_IMAGE_SIZE, (uint8_t *)UsbTsmPrivate.data_packet);
	uint8_t *data = (uint8_t *)&UsbTsmPrivate.data_packet->dat[0];
	store_le_uint16(&data[0], SGL_RGB_IMAGE_CH);
	store_le_uint16(&data[2], SGL_RGB_IMAGE_HEIGHT);
	store_le_uint16(&data[4], SGL_RGB_IMAGE_WIDTH);
	int offset = 6;
	for(int i=0; i<SGL_RGB_IMAGE_SIZE; i++)
	{
		data[offset++] = i;
	}
	
	printf("%s %d \n",__func__,__LINE__);
	UsbTsmPrivate.get_image = false;
	UsbTsmPrivate.aie_cal_flag = false;
	UsbTsmPrivate.ai_task_ctl = false;

	UsbTsmPrivate.image[IR_IMG].pixel_size = IR_IMAGE_PIXEL_SIZE;
	UsbTsmPrivate.image[IR_IMG].channel = IR_IMAGE_CH;
	UsbTsmPrivate.image[IR_IMG].height = IR_IMAGE_HEIGHT;
	UsbTsmPrivate.image[IR_IMG].width = IR_IMAGE_WIDTH;
	UsbTsmPrivate.image[IR_IMG].size = IR_IMAGE_SIZE;
	UsbTsmPrivate.image[IR_IMG].buf = (uint8_t *)&UsbTsmPrivate.data_packet->dat[6];

	UsbTsmPrivate.image[RGB_IMG].pixel_size = RGB_IMAGE_PIXEL_SIZE;
	UsbTsmPrivate.image[RGB_IMG].channel = RGB_IMAGE_CH;
	UsbTsmPrivate.image[RGB_IMG].height = RGB_IMAGE_HEIGHT;
	UsbTsmPrivate.image[RGB_IMG].width = RGB_IMAGE_WIDTH;
	UsbTsmPrivate.image[RGB_IMG].size = RGB_IMAGE_SIZE;
	UsbTsmPrivate.image[RGB_IMG].buf = (uint8_t *)&UsbTsmPrivate.data_packet->dat[6];

	UsbTsmPrivate.image[DEPTH_IMG].pixel_size = DEPTH_IMAGE_PIXEL_SIZE;
	UsbTsmPrivate.image[DEPTH_IMG].channel = DEPTH_IMAGE_CH;
	UsbTsmPrivate.image[DEPTH_IMG].height = DEPTH_IMAGE_HEIGHT;
	UsbTsmPrivate.image[DEPTH_IMG].width = DEPTH_IMAGE_WIDTH;
	UsbTsmPrivate.image[DEPTH_IMG].size = DEPTH_IMAGE_SIZE;
	UsbTsmPrivate.image[DEPTH_IMG].buf = (uint8_t *)&UsbTsmPrivate.data_packet->dat[6];

	printf("wait usb port %d to link~~\r\n", USB_PORT);	
	while(sud_FTemplIsConnected(USB_PORT) == 0); 
	printf("usb port %d linked\r\n", USB_PORT);	

	aos_task_new("AI_task", AI_task, NULL, 1024*20); 

	bin_buf = aos_malloc(128*1024);
	if(bin_buf == NULL)
	{
		return -1;
		printf("usbtsm_init falid, malloc failed\n");
	}
	
	printf("usbtsm_init success\n");
	return ret;
}

void ftempl_usb_task(void *paras)
{
	const char *task_name;
	task_name = aos_task_name();
	printf("task name: %s start!\r\n", task_name);
	printf("image usb demo start!\r\n");

	if(usbtsm_init() == -1)
	{
		printf("usb_task init error !!!\r\n");
		aos_task_exit(0); 
		return;
	}
	
    while(1)
	{
		//static unsigned int count = 0;
		//printf("task name: %s start%d!\r\n", task_name, count++);
		decode_protocal(); 
        aos_msleep(1);
    }
	return;
}




